#include "motorsTask.h"

#include "FreeRTOS.h"
#include "task.h"

#include "encoders.h"
#include <math.h>
static void motorsTask(void *pvParameters);
int sum(int *buffer);

void executeMotorsTask(general *pvParameter)
{
	xTaskCreate(motorsTask,(const signed portCHAR * const) "motorTask",motorsTask_stackSize,(void *)pvParameter,tskIDLE_PRIORITY+2,( xTaskHandle * ) NULL);
}


//Tarea para solo actualizar las vels en la esctructura global
static void motorsTask(void *pvParameters)
{	
	//convertion of parameters 
	general *globals;
	globals=(general *)pvParameters;

	unsigned int freq;
	portTickType actualTick;

	//variables for PI Control
	int errorL, errorR;

	unsigned char left, right;
	
	/*TODO: Variables para el control de velocidad
	static int errorStackL[8];
	static int errorStackR[8];
	static int errorSumL;
	static int errorSumR;
	
	static int *errorPtrL = errorStackL;
	static int *errorPtrR = errorStackR;
	*/
	
	//variables used for odometry
	
	//unsigned int posLeft, posRight;
	//static unsigned int lastPosLeft, lastPosRight;
	long change_left;
  long change_right;
  float transchange;
  float rotchange;
  float dx,dy;
  //static float x,y,yaw;
  
	
	for(;;)
	{		
		//get pos
		/*
		globals->gRobotState.posXAct = encoders_getStateLeft();
		globals->gRobotState.posYAct = encoders_getStateRight();
		
		if(globals->gRobotState.posXAct>= 1736)
			motors_setVel(0,MOTOR_LEFT);
		else
			motors_setVel(100,MOTOR_LEFT);
		
		if(globals->gRobotState.posYAct>= 1736)
			motors_setVel(0,MOTOR_RIGHT);
		else
			motors_setVel(100,MOTOR_RIGHT);
		
		*/

		//aquire the frequency for the task		
		
		freq=globals->gTimes.intervalMotors;
		
		/*
		//read actual velocities
		motors_getActualVels(&(globals->gVels),freq);
		
		//calculate error
		errorL = (globals->gVels.velLeftRef - globals->gVels.velLeftAct);
		errorR = (globals->gVels.velRightRef - globals->gVels.velRightAct);
		
		*errorPtrL = errorL;
		*errorPtrR = errorR;
		
		errorPtrL++;
		errorPtrR++;
		
		if(errorPtrL > errorStackL+7)
			errorPtrL = errorStackL;
		if(errorPtrR > errorStackR+7)
			errorPtrR = errorStackR;
		
		errorSumL = sum(errorStackL);
		errorSumR = sum(errorStackR);
		
		*/
		
		//First update position
		
		globals->gEncod.posLeft = encoders_getStateLeft();
		globals->gEncod.posRight = encoders_getStateRight();
		
		//calculate variations of position of wheels
		change_left = globals->gEncod.posLeft - globals->gEncod.lastPosLeft;
  	change_right = globals->gEncod.posRight - globals->gEncod.lastPosRight;
  
		//correction of encoders overflow
		if(change_left>60000)
			change_left = (globals->gEncod.posLeft-65535)-globals->gEncod.lastPosLeft;
	 	else if(change_left<-60000)
	 		change_left = (globals->gEncod.posLeft)+(65535-globals->gEncod.lastPosLeft);
	 	
	 	if(change_right>60000)
			change_right = (globals->gEncod.posRight-65535)-globals->gEncod.lastPosRight;
	 	else if(change_right<-60000)
	 		change_right = (globals->gEncod.posRight)+(65535-globals->gEncod.lastPosRight);
 	
	 	//update last positions
		globals->gEncod.lastPosLeft = globals->gEncod.posLeft;
		globals->gEncod.lastPosRight = globals->gEncod.posRight;

		transchange = (change_left + change_right) * NXBOT_ENCODER_RES / 2;
		rotchange = (change_left - change_right) * NXBOT_ENCODER_RES / NXBOT_WHEEL_DISTANCE;

		dx = transchange*cos(-globals->gRobotState.yaw - rotchange/2);
		dy = transchange*sin(-globals->gRobotState.yaw - rotchange/2);
		//dx = transchange*cos(-globals->gRobotState.yaw);
		//dy = transchange*sin(-globals->gRobotState.yaw);
		
		globals->gRobotState.posXAct += dx;
		globals->gRobotState.posYAct += dy;
		globals->gRobotState.yaw += rotchange;

		while (globals->gRobotState.yaw > 2*M_PI) 
			globals->gRobotState.yaw -= 2*M_PI;
		while (globals->gRobotState.yaw < 0) 
			globals->gRobotState.yaw += 2*M_PI;

		globals->gRobotState.orientation = globals->gRobotState.yaw;
		
		if(globals->gRobotState.orientation > M_PI)
  		globals->gRobotState.orientation -=2*M_PI;

  	globals->gRobotState.orientation *= -1;
		//set positions
		//globals->gRobotState.posXAct = encoders_getStateLeft();
		//globals->gRobotState.posYAct = encoders_getStateRight();
		
		
		//set velocities
		
		if(globals->gVels.velLeftRef>0)
			left = ENC_FORWARD;
		else if(globals->gVels.velLeftRef<0)
			left = ENC_REVERSE;
			
		if(globals->gVels.velRightRef>0)
			right = ENC_FORWARD;
		else if (globals->gVels.velRightRef<0)
			right = ENC_REVERSE;
		
		//First, define direction in odometers
		if(globals->gVels.velLeftRef || globals->gVels.velRightRef)  
			encoders_setDirection(left, right);
		//then set velocity in motors
		motors_setVel(globals->gVels.velLeftRef,MOTOR_LEFT);
		
		motors_setVel(globals->gVels.velRightRef,MOTOR_RIGHT);
		
		
		//time delay according to the encodersInterval value
		actualTick = xTaskGetTickCount();
		vTaskDelayUntil( &actualTick,(portTickType)freq);		
	}
}

int sum(int *buffer)
{
	int result;
	for(char i=0; i<8; i++)
		result += buffer[i];
	
	return result;
		
}
